#include <stdio.h>
#include <stdlib.h>
#include <sys/socket.h>
#include <unistd.h>
#include <netdb.h>
#include <errno.h>
#include <string.h>
#include <string>
#include <sstream>
#include <algorithm>
#include "simplemap.hpp"
#include "roswrapper.hpp"

#define SHUTDOWN "SHUTDOWN"
#define INIT "INIT"
#define NEWCONNECTION "HI\n"
#define CLOSECONNECTION "BYE\n"
#define LOOK "SCAN"
#define MOVE "MOVE"
#define SUCCESS "OK\n"
#define FAILURE "INVALID\n"
#define FAILURE2 "HUH\n"
#define GOAL "GOAL\n"

#define NORTHCMD "NORTH"
#define SOUTHCMD "SOUTH"
#define EASTCMD "EAST"
#define WESTCMD "WEST"

#define LISTENING_DEPTH 1
#define BUF_SIZE 1024

const int DirectionCount = 4;
enum Direction { 	NORTH 	= 0,
				EAST 	= 1,
				SOUTH 	= 2,
				WEST 	= 3,
};

Direction orientation = NORTH;


int setupSocket(unsigned long serverPort) {
	struct sockaddr_in sock;
	int socket_descriptor;

	if ((socket_descriptor = socket(AF_INET, SOCK_STREAM, 0)) == -1) {
		perror("socket");
		return -1;
	}

	sock.sin_family = AF_INET;
	sock.sin_addr.s_addr = INADDR_ANY;
	sock.sin_port = htons(serverPort);

	if(bind(socket_descriptor, (struct sockaddr*)&sock, sizeof(sock)) == -1) {
		perror("bind");
		return -1;
	}

	if (listen(socket_descriptor, LISTENING_DEPTH) == -1) {
		perror("listen");
		return -1;
	}
	return socket_descriptor;
}

std::string toUpper(const std::string &str) {
	std::string newStr(str);
	std::transform(newStr.begin(), newStr.end(), newStr.begin(), ::toupper);
	return newStr;
}

int getRequiredRotations(Direction currentDirection, Direction newDirection) {
	int rotation = (int)currentDirection - (int)newDirection;

	if(rotation == 3) rotation = -1;
	else if(rotation == -3) rotation = 1;

	return rotation;
}

bool lookCommand(int socket_descriptor, const SimpleMap &graph, RosWrapper &rw) {
	bool success = true;

	ROS_INFO("SCAN COMMAND");

	//rotate all the way around for show
	for(int i = 0; i < 4; i++) {
		ROS_INFO("ROTATE");
//		if(!(success = rw.rotate(1))) break;
		//the direction doesn't matter here
		orientation = (Direction)(((int)orientation + 1) % 4);
		if((int)orientation < 0) orientation = (Direction)((int)orientation + 4);
	}

	if(success) {
		std::vector<std::string> clear = graph.getFreeNeighborStrings();
		stringstream ss;
		for(unsigned int i = 0; i < clear.size(); i++)
			if(i < (clear.size() - 1))
				ss << clear[i] << " ";
			else
				ss << clear[i] << "\n";
		std::string clearString = ss.str();
		ROS_INFO("sending %s", clearString.c_str());

		send(socket_descriptor, clearString.c_str(), sizeof(char) * clearString.length(), 0);

		ROS_INFO("SENT");
	}

	if(!success) {
		ROS_ERROR("LOOK ERROR");
		send(socket_descriptor, FAILURE, sizeof(char) * strlen(FAILURE), 0);
	}

	return success;
}

const char* getString(int d) {
	switch(d) {
	case NORTH: return "NORTH";
	case EAST: return "EAST";
	case SOUTH: return "SOUTH";
	default: return "WEST";
	}
}

bool moveCommand(int socket_descriptor, Direction newDirection, SimpleMap &graph, RosWrapper &rw) {
	bool success = true;

	ROS_INFO("MOVE COMMAND, currnetly facing %s -- move %s", getString((int)orientation), getString((int)newDirection));

	std::string directionString;
	if(newDirection == NORTH) directionString = "NORTH";
	else if(newDirection == SOUTH) directionString = "SOUTH";
	else if(newDirection == EAST) directionString = "EAST";
	else directionString = "WEST";

	std::vector<std::string> blocked = graph.getBlockedNeighborStrings();
	for(unsigned int i = 0; i < blocked.size(); i++) {
		ROS_ERROR("%s", blocked[i].c_str());
		if(directionString.compare(blocked[i]) == 0) {
			ROS_INFO("BLOCKED!");
			success = false;
			break;
		}
	}

	if(success) {
		int rotations = getRequiredRotations(orientation, newDirection);
		int howmany = abs(rotations);
		int direction = howmany == 0 ? 0 : rotations / howmany;
		ROS_INFO("facing %d going to %d needs %d rotations in %d direction", orientation, newDirection, howmany, direction);


		ROS_INFO("starting orientation: %s", getString((int)orientation));
		for(int i = 0; i < howmany; i++) {
			ROS_INFO("ROTATE %d", direction);
			if(!(success = rw.rotate(direction))) break;
			orientation = (Direction)(((int)orientation - direction) % 4);
			if((int)orientation < 0) orientation = (Direction)((int)orientation + 4);
			ROS_INFO("new orientation: %s", getString((int)orientation));
		}

		if(success) {
			ROS_INFO("MOVING");
			success = rw.moveFoward();
			ROS_INFO("MOVED");
		}
	}

	if(success)
		success = graph.updatePosition(directionString);

	if(success) {
		if(graph.atGoal())
			send(socket_descriptor, GOAL, sizeof(char) * strlen(GOAL), 0);
		else
			send(socket_descriptor, SUCCESS, sizeof(char) * strlen(SUCCESS), 0);
	}
	else {
		ROS_ERROR("MOVE ERROR");
		send(socket_descriptor, FAILURE, sizeof(char) * strlen(FAILURE), 0);
	}

	return success;
}

int main(int argc, char* argv[]) {
	ros::init(argc, argv, "maze_runner");
	ros::NodeHandle n("maze");

	int port = 2222;
	double dxy = 0.6096;
	double dr = M_PI / 2;
	std::string filename = "maze.dat";

	std::string param;

	if(n.getParam("/maze/port", port)) ROS_INFO("using port: %u", port);
	else { port = 2222; ROS_WARN("port not set, using: %u", port); }

	if(n.getParam("/maze/dxy_discr", dxy)) ROS_INFO("using dxy: %f", dxy);
	else ROS_WARN("dxy_discr not set, using: %f", dxy);

	if(n.getParam("/maze/dr_discr", dr)) ROS_INFO("using dr: %f", dr);
	else ROS_WARN("dr_discr not set, using: %f", dr);

	if(n.getParam("/maze/init_orien", param)) orientation = NORTH;
	else ROS_WARN("init_orien not set, using: NORTH");

	if(n.getParam("/maze/graph_file", filename)) ROS_INFO("using graph file: %s", filename.c_str());
	else ROS_WARN("graph_file not set, using: %s", filename.c_str());

	SimpleMap graph(filename);

	RosWrapper rw(dxy, dr);

	//ugh at some point readt this from the parameter server
	unsigned long serverPort = port;

	int socket_descriptor = setupSocket(serverPort);
	if(socket_descriptor < 0) { fprintf(stderr, "could not create socket descriptor\n"); return 1; }

	char buffer[BUF_SIZE];

	while(true) {
		int current_sd = -1;

		if((current_sd = accept(socket_descriptor, NULL, 0)) < 0) {
			perror("accept");
			break;
		}
		else {
			ROS_INFO("CONNECTED");
			send(current_sd, NEWCONNECTION, sizeof(char) * strlen(NEWCONNECTION), 0);

			while(true) {
				if(recv(current_sd, buffer, BUF_SIZE, 0) < 1) {
					fprintf(stderr, "recv error or got no bytes\n");
					break;
				}
				std::string command(buffer);
				std::string upperCaseCommand = toUpper(command);

				if(upperCaseCommand.find(SHUTDOWN) != std::string::npos) {
					ROS_INFO("shutdown request %s\n", upperCaseCommand.c_str());
					close(current_sd);
					goto shutdown;
				}
				else if(upperCaseCommand.find(CLOSECONNECTION) != std::string::npos) {
					ROS_INFO("disconnect request %s\n", upperCaseCommand.c_str());
					break;
				}
				else if(upperCaseCommand.find(LOOK) != std::string::npos) {
					if(!lookCommand(current_sd, graph, rw)) {
						ROS_ERROR("command failed: %s", buffer);
						break;
					}
				}
				else if(upperCaseCommand.find(NORTHCMD) != std::string::npos) {
					if(!moveCommand(current_sd, NORTH, graph, rw)) {
						ROS_ERROR("command failed: %s", buffer);
						break;
					}
				}
				else if(upperCaseCommand.find(SOUTHCMD) != std::string::npos) {
					if(!moveCommand(current_sd, SOUTH, graph, rw)) {
						ROS_ERROR("command failed: %s", buffer);
						break;
					}
				}
				else if(upperCaseCommand.find(EASTCMD) != std::string::npos) {
					if(!moveCommand(current_sd, EAST, graph, rw)) {
						ROS_ERROR("command failed: %s", buffer);
						break;
					}
				}
				else if(upperCaseCommand.find(WESTCMD) != std::string::npos) {
					if(!moveCommand(current_sd, WEST, graph, rw)) {
						ROS_ERROR("command failed: %s", buffer);
						break;
					}
				}
				else {
					ROS_ERROR("unrecognized command: %s", buffer);
					send(current_sd, FAILURE2, sizeof(char) * strlen(FAILURE2), 0);
					break;
				}
				if(graph.atGoal()) {
					graph.swapStartAndGoal();
				}
			}
			close(current_sd);
		}
	}

shutdown:
	ROS_WARN("maze runner closing");
	close(socket_descriptor);

	return 0;
}
